home *** CD-ROM | disk | FTP | other *** search
/ BBS in a Box 3 / BBS in a box - Trilogy III.iso / Files / Prog / D-G / GemsI / Original / FitDigitizedCurves.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-06-16  |  14.0 KB  |  529 lines  |  [TEXT/MPS ]
  1. /*
  2. An Algorithm for Automatically Fitting Digitized Curves
  3. by Philip J. Schneider
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6.  
  7. /*  fit_cubic.c    */                                    
  8. /*    Piecewise cubic fitting code    */
  9.  
  10. #include "GraphicsGems.h"                    
  11. #include <stdio.h>
  12. #include <malloc.h>
  13. #include <math.h>
  14.  
  15. typedef Point2 *BezierCurve;
  16.  
  17. /* Forward declarations */
  18.         void        FitCurve();
  19. static    void        FitCubic();
  20. static    double        *Reparameterize();
  21. static    double        NewtonRaphsonRootFind();
  22. static    Point2        Bezier();
  23. static    double         B0(), B1(), B2(), B3();
  24. static    Vector2        ComputeLeftTangent();
  25. static    Vector2        ComputeRightTangent();
  26. static    Vector2        ComputeCenterTangent();
  27. static    double        ComputeMaxError();
  28. static    double        *ChordLengthParameterize();
  29. static    BezierCurve    GenerateBezier();
  30. static    Vector2        V2AddII();
  31. static    Vector2        V2ScaleII();
  32. static    Vector2        V2Sub();
  33.  
  34. #define MAXPOINTS    1000        /* The most points you can have */
  35.  
  36. #ifdef TESTMODE
  37.  
  38. /*
  39.  *  main:
  40.  *    Example of how to use the curve-fitting code.  Given an array
  41.  *   of points and a tolerance (squared error between points and 
  42.  *    fitted curve), the algorithm will generate a piecewise
  43.  *    cubic Bezier representation that approximates the points.
  44.  *    When a cubic is generated, the routine "DrawBezierCurve"
  45.  *    is called, which outputs the Bezier curve just created
  46.  *    (arguments are the degree and the control points, respectively).
  47.  *    Users will have to implement this function themselves     
  48.  *   ascii output, etc. 
  49.  *
  50.  */    
  51. main()
  52. {
  53.     static Point2 d[7] = {    /*  Digitized points */
  54.     { 0.0, 0.0 },
  55.     { 0.0, 0.5 },
  56.     { 1.1, 1.4 },
  57.     { 2.1, 1.6 },
  58.     { 3.2, 1.1 },
  59.     { 4.0, 0.2 },
  60.     { 4.0, 0.0 },
  61.     };
  62.     double    error = 4.0;        /*  Squared error */
  63.     FitCurve(d, 7, error);        /*  Fit the Bezier curves */
  64. }
  65. #endif                         /* TESTMODE */
  66.  
  67. /*
  68.  *  FitCurve :
  69.  *      Fit a Bezier curve to a set of digitized points 
  70.  */
  71. void FitCurve(d, nPts, error)
  72.     Point2    *d;            /*  Array of digitized points    */
  73.     int        nPts;        /*  Number of digitized points    */
  74.     double    error;        /*  User-defined error squared    */
  75. {
  76.     Vector2    tHat1, tHat2;    /*  Unit tangent vectors at endpoints */
  77.  
  78.     tHat1 = ComputeLeftTangent(d, 0);
  79.     tHat2 = ComputeRightTangent(d, nPts - 1);
  80.     FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
  81. }
  82.  
  83.  
  84.  
  85. /*
  86.  *  FitCubic :
  87.  *      Fit a Bezier curve to a (sub)set of digitized points
  88.  */
  89. static void FitCubic(d, first, last, tHat1, tHat2, error)
  90.     Point2    *d;            /*  Array of digitized points */
  91.     int        first, last;    /* Indices of first and last pts in region */
  92.     Vector2    tHat1, tHat2;    /* Unit tangent vectors at endpoints */
  93.     double    error;        /*  User-defined error squared       */
  94. {
  95.     BezierCurve    bezCurve; /*Control points of fitted Bezier curve*/
  96.     double    *u;        /*  Parameter values for point  */
  97.     double    *uPrime;    /*  Improved parameter values */
  98.     double    maxError;    /*  Maximum fitting error     */
  99.     int        splitPoint;    /*  Point to split point set at     */
  100.     int        nPts;        /*  Number of points in subset  */
  101.     double    iterationError; /*Error below which you try iterating  */
  102.     int        maxIterations = 4; /*  Max times to try iterating  */
  103.     Vector2    tHatCenter;       /* Unit tangent vector at splitPoint */
  104.     int        i;        
  105.  
  106.     iterationError = error * error;
  107.     nPts = last - first + 1;
  108.  
  109.     /*  Use heuristic if region only has two points in it */
  110.     if (nPts == 2) {
  111.         double dist = V2DistanceBetween2Points(&d[last], &d[first]) /             3.0;
  112.  
  113.         bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
  114.         bezCurve[0] = d[first];
  115.         bezCurve[3] = d[last];
  116.         V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
  117.         V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
  118.         DrawBezierCurve(3, bezCurve);
  119.         return;
  120.     }
  121.  
  122.     /*  Parameterize points, and attempt to fit curve */
  123.     u = ChordLengthParameterize(d, first, last);
  124.     bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
  125.  
  126.     /*  Find max deviation of points to fitted curve */
  127.     maxError = ComputeMaxError(d, first, last, bezCurve, u,                     &splitPoint);
  128.     if (maxError < error) {
  129.         DrawBezierCurve(3, bezCurve);
  130.         return;
  131.     }
  132.  
  133.  
  134.     /*  If error not too large, try some reparameterization  */
  135.     /*  and iteration */
  136.     if (maxError < iterationError) {
  137.         for (i = 0; i < maxIterations; i++) {
  138.             uPrime = Reparameterize(d, first, last, u, bezCurve);
  139.             bezCurve = GenerateBezier(d, first, last, uPrime, tHat1,                 tHat2);
  140.             maxError = ComputeMaxError(d, first, last,
  141.                        bezCurve, uPrime, &splitPoint);
  142.             if (maxError < error) {
  143.             DrawBezierCurve(3, bezCurve);
  144.             return;
  145.         }
  146.         free((char *)u);
  147.         u = uPrime;
  148.     }
  149. }
  150.  
  151.     /* Fitting failed -- split at max error point and fit recursively */
  152.     tHatCenter = ComputeCenterTangent(d, splitPoint);
  153.     FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
  154.     V2Negate(&tHatCenter);
  155.     FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
  156. }
  157.  
  158.  
  159. /*
  160.  *  GenerateBezier :
  161.  *  Use least-squares method to find Bezier control points for region.
  162.  *
  163.  */
  164. static BezierCurve  GenerateBezier(d, first, last, uPrime, tHat1,                     tHat2)
  165.     Point2    *d;            /*  Array of digitized points    */
  166.     int        first, last;        /*  Indices defining region    */
  167.     double    *uPrime;        /*  Parameter values for region */
  168.     Vector2    tHat1, tHat2;    /*  Unit tangents at endpoints    */
  169. {
  170.     int     i;
  171.     Vector2     A[MAXPOINTS][2];    /* Precomputed rhs for eqn    */
  172.     int     nPts;            /* Number of pts in sub-curve */
  173.     double     C[2][2];            /* Matrix C            */
  174.     double     X[2];            /* Matrix X            */
  175.     double     det_C0_C1,        /* Determinants of matrices    */
  176.                det_C0_X,
  177.                det_X_C1;
  178.     double     alpha_l,        /* Alpha values, left and right    */
  179.                alpha_r;
  180.     Vector2     tmp;            /* Utility variable        */
  181.     BezierCurve    bezCurve;    /* RETURN bezier curve ctl pts    */
  182.  
  183.     bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
  184.     nPts = last - first + 1;
  185.  
  186.  
  187.     /* Compute the A's    */
  188.     for (i = 0; i < nPts; i++) {
  189.         Vector2        v1, v2;
  190.         v1 = tHat1;
  191.         v2 = tHat2;
  192.         V2Scale(&v1, B1(uPrime[i]));
  193.         V2Scale(&v2, B2(uPrime[i]));
  194.         A[i][0] = v1;
  195.         A[i][1] = v2;
  196.     }
  197.  
  198.     /* Create the C and X matrices    */
  199.     C[0][0] = 0.0;
  200.     C[0][1] = 0.0;
  201.     C[1][0] = 0.0;
  202.     C[1][1] = 0.0;
  203.     X[0]    = 0.0;
  204.     X[1]    = 0.0;
  205.  
  206.     for (i = 0; i < nPts; i++) {
  207.         C[0][0] += V2Dot(&A[i][0], &A[i][0]);
  208.         C[0][1] += V2Dot(&A[i][0], &A[i][1]);
  209. /*                    C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/    
  210.         C[1][0] = C[0][1];
  211.         C[1][1] += V2Dot(&A[i][1], &A[i][1]);
  212.  
  213.         tmp = V2Sub(d[first + i],
  214.             V2AddII(
  215.               V2ScaleII(d[first], B0(uPrime[i])),
  216.                 V2AddII(
  217.                       V2ScaleII(d[first], B1(uPrime[i])),
  218.                             V2AddII(
  219.                               V2ScaleII(d[last], B2(uPrime[i])),
  220.                                 V2ScaleII(d[last], B3(uPrime[i]))))));
  221.     
  222.  
  223.     X[0] += V2Dot(&A[i][0], &tmp);
  224.     X[1] += V2Dot(&A[i][1], &tmp);
  225.     }
  226.  
  227.     /* Compute the determinants of C and X    */
  228.     det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
  229.     det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
  230.     det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];
  231.  
  232.     /* Finally, derive alpha values    */
  233.     if (det_C0_C1 == 0.0) {
  234.         det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
  235.     }
  236.     alpha_l = det_X_C1 / det_C0_C1;
  237.     alpha_r = det_C0_X / det_C0_C1;
  238.  
  239.  
  240.     /*  If alpha negative, use the Wu/Barsky heuristic (see text) */
  241.     if (alpha_l < 0.0 || alpha_r < 0.0) {
  242.         double    dist = V2DistanceBetween2Points(&d[last], &d[first]) /                         3.0;
  243.  
  244.         bezCurve[0] = d[first];
  245.         bezCurve[3] = d[last];
  246.         V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
  247.         V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
  248.         return (bezCurve);
  249.     }
  250.  
  251.     /*  First and last control points of the Bezier curve are */
  252.     /*  positioned exactly at the first and last data points */
  253.     /*  Control points 1 and 2 are positioned an alpha distance out */
  254.     /*  on the tangent vectors, left and right, respectively */
  255.     bezCurve[0] = d[first];
  256.     bezCurve[3] = d[last];
  257.     V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
  258.     V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
  259.     return (bezCurve);
  260. }
  261.  
  262.  
  263. /*
  264.  *  Reparameterize:
  265.  *    Given set of points and their parameterization, try to find
  266.  *   a better parameterization.
  267.  *
  268.  */
  269. static double *Reparameterize(d, first, last, u, bezCurve)
  270.     Point2    *d;            /*  Array of digitized points    */
  271.     int        first, last;        /*  Indices defining region    */
  272.     double    *u;            /*  Current parameter values    */
  273.     BezierCurve    bezCurve;    /*  Current fitted curve    */
  274. {
  275.     int     nPts = last-first+1;    
  276.     int     i;
  277.     double    *uPrime;        /*  New parameter values    */
  278.  
  279.     uPrime = (double *)malloc(nPts * sizeof(double));
  280.     for (i = first; i <= last; i++) {
  281.         uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-                            first]);
  282.     }
  283.     return (uPrime);
  284. }
  285.  
  286.  
  287.  
  288. /*
  289.  *  NewtonRaphsonRootFind :
  290.  *    Use Newton-Raphson iteration to find better root.
  291.  */
  292. static double NewtonRaphsonRootFind(Q, P, u)
  293.     BezierCurve    Q;            /*  Current fitted curve    */
  294.     Point2         P;            /*  Digitized point        */
  295.     double         u;            /*  Parameter value for "P"    */
  296. {
  297.     double         numerator, denominator;
  298.     Point2         Q1[3], Q2[2];    /*  Q' and Q''            */
  299.     Point2        Q_u, Q1_u, Q2_u;    /*u evaluated at Q, Q', & Q''    */
  300.     double         uPrime;            /*  Improved u            */
  301.     int         i;
  302.     
  303.     /* Compute Q(u)    */
  304.     Q_u = Bezier(3, Q, u);
  305.     
  306.     /* Generate control vertices for Q'    */
  307.     for (i = 0; i <= 2; i++) {
  308.         Q1[i].x = (Q[i+1].x - Q[i].x) * 3.0;
  309.         Q1[i].y = (Q[i+1].y - Q[i].y) * 3.0;
  310.     }
  311.     
  312.     /* Generate control vertices for Q'' */
  313.     for (i = 0; i <= 1; i++) {
  314.         Q2[i].x = (Q1[i+1].x - Q1[i].x) * 2.0;
  315.         Q2[i].y = (Q1[i+1].y - Q1[i].y) * 2.0;
  316.     }
  317.     
  318.     /* Compute Q'(u) and Q''(u)    */
  319.     Q1_u = Bezier(2, Q1, u);
  320.     Q2_u = Bezier(1, Q2, u);
  321.     
  322.     /* Compute f(u)/f'(u) */
  323.     numerator = (Q_u.x - P.x) * (Q1_u.x) + (Q_u.y - P.y) * (Q1_u.y);
  324.     denominator = (Q1_u.x) * (Q1_u.x) + (Q1_u.y) * (Q1_u.y) +
  325.                     (Q_u.x - P.x) * (Q2_u.x) + (Q_u.y - P.y) * (Q2_u.y);
  326.     
  327.     /* u = u - f(u)/f'(u) */
  328.     uPrime = u - (numerator/denominator);
  329.     return (uPrime);
  330. }
  331.  
  332.     
  333.                
  334. /*
  335.  *  Bezier :
  336.  *      Evaluate a Bezier curve at a particular parameter value
  337.  * 
  338.  */
  339. static Point2 Bezier(degree, V, t)
  340.     int        degree;        /* The degree of the bezier curve    */
  341.     Point2     *V;        /* Array of control points        */
  342.     double     t;        /* Parametric value to find point for    */
  343. {
  344.     int     i, j;        
  345.     Point2     Q;            /* Point on curve at parameter t    */
  346.     Point2     *Vtemp;        /* Local copy of control points        */
  347.  
  348.     /* Copy array    */
  349.     Vtemp = (Point2 *)malloc((unsigned)((degree+1) 
  350.                 * sizeof (Point2)));
  351.     for (i = 0; i <= degree; i++) {
  352.         Vtemp[i] = V[i];
  353.     }
  354.  
  355.     /* Triangle computation    */
  356.     for (i = 1; i <= degree; i++) {    
  357.         for (j = 0; j <= degree-i; j++) {
  358.             Vtemp[j].x = (1.0 - t) * Vtemp[j].x + t * Vtemp[j+1].x;
  359.             Vtemp[j].y = (1.0 - t) * Vtemp[j].y + t * Vtemp[j+1].y;
  360.         }
  361.     }
  362.  
  363.     Q = Vtemp[0];
  364.     free((char *)Vtemp);
  365.     return Q;
  366. }
  367.  
  368.  
  369. /*
  370.  *  B0, B1, B2, B3 :
  371.  *    Bezier multipliers
  372.  */
  373. static double B0(u)
  374.     double    u;
  375. {
  376.     double tmp = 1.0 - u;
  377.     return (tmp * tmp * tmp);
  378. }
  379.  
  380.  
  381. static double B1(u)
  382.     double    u;
  383. {
  384.     double tmp = 1.0 - u;
  385.     return (3 * u * (tmp * tmp));
  386. }
  387.  
  388. static double B2(u)
  389.     double    u;
  390. {
  391.     double tmp = 1.0 - u;
  392.     return (3 * u * u * tmp);
  393. }
  394.  
  395. static double B3(u)
  396.     double    u;
  397. {
  398.     return (u * u * u);
  399. }
  400.  
  401.  
  402.  
  403. /*
  404.  * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
  405.  *Approximate unit tangents at endpoints and "center" of digitized curve
  406.  */
  407. static Vector2 ComputeLeftTangent(d, end)
  408.     Point2    *d;            /*  Digitized points*/
  409.     int        end;        /*  Index to "left" end of region */
  410. {
  411.     Vector2    tHat1;
  412.     tHat1 = V2Sub(d[end+1], d[end]);
  413.     tHat1 = *V2Normalize(&tHat1);
  414.     return tHat1;
  415. }
  416.  
  417. static Vector2 ComputeRightTangent(d, end)
  418.     Point2    *d;            /*  Digitized points        */
  419.     int        end;        /*  Index to "right" end of region */
  420. {
  421.     Vector2    tHat2;
  422.     tHat2 = V2Sub(d[end-1], d[end]);
  423.     tHat2 = *V2Normalize(&tHat2);
  424.     return tHat2;
  425. }
  426.  
  427.  
  428. static Vector2 ComputeCenterTangent(d, center)
  429.     Point2    *d;            /*  Digitized points            */
  430.     int        center;        /*  Index to point inside region    */
  431. {
  432.     Vector2    V1, V2, tHatCenter;
  433.  
  434.     V1 = V2Sub(d[center-1], d[center]);
  435.     V2 = V2Sub(d[center], d[center+1]);
  436.     tHatCenter.x = (V1.x + V2.x)/2.0;
  437.     tHatCenter.y = (V1.y + V2.y)/2.0;
  438.     tHatCenter = *V2Normalize(&tHatCenter);
  439.     return tHatCenter;
  440. }
  441.  
  442.  
  443. /*
  444.  *  ChordLengthParameterize :
  445.  *    Assign parameter values to digitized points 
  446.  *    using relative distances between points.
  447.  */
  448. static double *ChordLengthParameterize(d, first, last)
  449.     Point2    *d;                /* Array of digitized points */
  450.     int        first, last;        /*  Indices defining region    */
  451. {
  452.     int        i;    
  453.     double    *u;            /*  Parameterization        */
  454.  
  455.     u = (double *)malloc((unsigned)(last-first+1) *                     sizeof(double));
  456.  
  457.     u[0] = 0.0;
  458.     for (i = first+1; i <= last; i++) {
  459.         u[i-first] = u[i-first-1] +
  460.                           V2DistanceBetween2Points(&d[i], &d[i-1]);
  461.     }
  462.  
  463.     for (i = first + 1; i <= last; i++) {
  464.         u[i-first] = u[i-first] / u[last-first];
  465.     }
  466.  
  467.     return(u);
  468. }
  469.  
  470.  
  471.  
  472.  
  473. /*
  474.  *  ComputeMaxError :
  475.  *    Find the maximum squared distance of digitized points
  476.  *    to fitted curve.
  477. */
  478. static double ComputeMaxError(d, first, last, bezCurve, u,                         splitPoint)
  479.     Point2    *d;            /*  Array of digitized points    */
  480.     int        first, last;        /*  Indices defining region    */
  481.     BezierCurve    bezCurve;        /*  Fitted Bezier curve        */
  482.     double    *u;            /*  Parameterization of points    */
  483.     int        *splitPoint;        /*  Point of maximum error    */
  484. {
  485.     int        i;
  486.     double    maxDist;        /*  Maximum error        */
  487.     double    dist;        /*  Current error        */
  488.     Point2    P;            /*  Point on curve        */
  489.     Vector2    v;            /*  Vector from point to curve    */
  490.  
  491.     *splitPoint = (last - first + 1)/2;
  492.     maxDist = 0.0;
  493.     for (i = first + 1; i < last; i++) {
  494.         P = Bezier(3, bezCurve, u[i-first]);
  495.         v = V2Sub(P, d[i]);
  496.         dist = V2SquaredLength(&v);
  497.         if (dist >= maxDist) {
  498.             maxDist = dist;
  499.             *splitPoint = i;
  500.         }
  501.     }
  502.     return (maxDist);
  503. }
  504. static Vector2 V2AddII(a, b)
  505.     Vector2 a, b;
  506. {
  507.     Vector2    c;
  508.     c.x = a.x + b.x;  c.y = a.y + b.y;
  509.     return (c);
  510. }
  511. static Vector2 V2ScaleII(v, s)
  512.     Vector2    v;
  513.     double    s;
  514. {
  515.     Vector2 result;
  516.     result.x = v.x * s; result.y = v.y * s;
  517.     return (result);
  518. }
  519.  
  520. static Vector2 V2Sub(a, b)
  521.     Vector2    a, b;
  522. {
  523.     Vector2    c;
  524.     c.x = a.x - b.x; c.y = a.y - b.y;
  525.     return (c);
  526. }
  527.  
  528.  
  529.